#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将发布turtle1/cmd_vel话题，消息类型geometry_msgs::Twist

import rospy
import serial
from text_pkg.msg import cmd_vel

def publisher():
	# ROS节点初始化
    rospy.init_node('ROS')

	# 创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10
    cmd_vel_pub = rospy.Publisher('cmd_vel',cmd_vel, queue_size=10)

	#设置循环的频率
    rate = rospy.Rate(10)
    #print "s:"
    #pwm=input()
    print "pwm1:"
    pwm1=input()
    
    #while not rospy.is_shutdown():
    while pwm1!=-1:
		# 初始化geometry_msgs::Twist类型的消息
	#print "b:"
        #back=input()
        print "pwm2:"
        pwm2=input()
        print "back1:"
        back1=input()
        print "back2:"
        back2=input()
        print "quit1:"
        quit1=input()
        print "quit2:"
        quit2=input()
        vel = cmd_vel()
        vel.pwm1=pwm1;
        vel.back1=back1;
        vel.quit1=quit1;
        vel.pwm2=pwm2;
        vel.back2=back2;
        vel.quit2=quit2;
        
		# 发布消息
        cmd_vel_pub.publish(vel)
    	rospy.loginfo("ROS Publsh [p1:%d,p2:%d,b1:%d,b2:%d,q1:%d,q2:%d]", 
				vel.pwm1,vel.pwm2,vel.back1,vel.back2,vel.quit1,vel.quit2)

		# 按照循环频率延时
	#print "s:"
        #pwm=input()
        print "pwm1:"
        pwm1=input()
        rate.sleep()
    if pwm1==-1:
	vel = cmd_vel()
        vel.pwm1=100;
        vel.back1=0;
        vel.quit1=1;
        vel.pwm2=100;
        vel.back2=1;
        vel.quit2=1;
        cmd_vel_pub.publish(vel)

if __name__ == '__main__':
    try:
        publisher()
    except rospy.ROSInterruptException:
        pass


